#include "PfMap.h"

#include <iostream>
#include <fstream>
#include <string>
#include <deque>

static bool CheckStreamToken(std::istream & in, const std::string & validToken)
{
	std::string token;
	in >> token;
	if(token == validToken)
	{
		return true;
	}
	else
	{ 
		std::cout << "Load failed: Invalid format (Expected '" << validToken;
		std::cout << "', read '" << token << "')" << std::endl; 
		return false; 
	}
}

static bool CheckPowerOfTwo(int inValue, int & outPower)
{
	for(outPower = 0; outPower < 30; ++outPower)
	{
		if((1 << outPower) == inValue)
		{
			return true;
		}
	}
	return false;
}

bool PfMap::Load(const char * filename)
{
	std::ifstream in(filename);
	if(in)
	{
		if(!CheckStreamToken(in, "type")) return false;
		if(!CheckStreamToken(in, "octile")) return false;
		if(!CheckStreamToken(in, "height")) return false;
		in >> height;
		if(in.bad() || height < 1)
		{
			std::cout << "Load failed: Invalid dimensions (Height must be a positive integer)" << std::endl;
			return false;
		}
		if(!CheckStreamToken(in, "width")) return false;
		in >> width; 
		if(in.bad() || width < 1 || !CheckPowerOfTwo(width,wshift))
		{
			std::cout << "Load failed: Invalid dimensions (Width must be a positive integer power of two)" << std::endl;
			return false;
		}
		if(!CheckStreamToken(in, "map")) return false;
		
		tiles.resize(width*height);
		for(int y(0); y<height; ++y)
		{
			std::string line; in >> line;
			if(in.bad() || static_cast<int>(line.length()) < width) 
			{ 
				std::cout << "Load failed: Not enough data." << std::endl;
				return false;
			}
			for(int x(0); x<width; ++x)
			{
				(*this)(x,y) = (line[x] == '@') ? TILE_OBSTRUCTION : TILE_SPACE;
			}
		}
		std::cout << "Map loaded from " << filename << std::endl;
		return true;
	}
	else
	{
		std::cout << "Load failed: Unable to open " << filename << std::endl;
		return false;
	}
}

void PfMap::SaveGraph(const char * filename)
{
	std::ofstream out(filename);
	if(out)
	{
		std::cout << "Saving to " << filename << "..." << std::endl;
		// Map regions
		out << width << " " << height << std::endl;
		for(int y(0); y<height; ++y)
		{
			for(int x(0); x<width; ++x)
			{
				out << (int)At(x,y) << " ";
			}
			out << std::endl;
		}
		// Region data
		out << regions.size() << std::endl;
		for(size_t i(0); i<regions.size(); ++i)
		{
			const Region & region(regions[i]);
			out << region.borderVertices.size() << " ";
			for(size_t j(0); j<region.borderVertices.size(); ++j)
			{
				out << region.borderVertices[j] << " ";
			}
			out << std::endl;
		}
		// Border vertices
		out << borders.size() << std::endl;
		for(size_t i(0); i<borders.size(); ++i)
		{
			const Border & border(borders[i]);
			out << border.start.x << " " << border.start.y << " ";
			out << border.end.x << " " << border.end.y << std::endl;
		}
		std::cout << "Finished." << std::endl;
	}
	else
	{
		std::cout << "Cannot write to " << filename << std::endl;
	}
}

void PfMap::MakeRegions(const std::vector<PfSegment> & segBorders)
{
	for(int y(0); y<height; ++y)
	{
		for(int x(0); x<width; ++x)
		{
			Tile & tile((*this)(x,y));
			if(tile != TILE_OBSTRUCTION) tile = TILE_SPACE;
		}
	}

	borders.clear();
	for(size_t i(0); i<segBorders.size(); ++i)
	{
		SetLine(segBorders[i]);
	}

	Tile region(0);
	for(int y(0); y<height; ++y)
	{
		for(int x(0); x<width; ++x)
		{
			if(At(x,y) == TILE_SPACE)
			{
				FloodFill(x,y,region++);
			}
		}
	}
	numRegions = static_cast<int>(region);

	for(size_t i(0); i<borders.size(); ++i)
	{
		GenBorder(i);
	}

	// Generate a list of border vertices for each region
	regions.resize(numRegions);
	for(size_t i(0); i<numRegions; ++i)
	{
		GetRegion(i).borderVertices.clear();
	}
	for(size_t i(0); i<borders.size(); ++i)
	{
		GetRegion(GetBorder(i).regionA).borderVertices.push_back(i);
		GetRegion(GetBorder(i).regionB).borderVertices.push_back(i);
	}

	// Initialise our graph with no edges
	edges.resize(borders.size()*borders.size());
	for(size_t i(0); i<borders.size(); ++i)
	{
		for(size_t j(0); j<borders.size(); ++j)
		{
			Edge & edge(GetEdge(i,j));
			edge.connected	= false;
			edge.cost		= 100000000;
			edge.successor	= 255;
		}
	}

	// For each region, make a clique from its border vertices
	for(size_t i(0); i<regions.size(); ++i)
	{
		const Region & region(GetRegion(i));
		for(size_t j(0); j<region.borderVertices.size(); ++j)
		{
			for(size_t k(j+1); k<region.borderVertices.size(); ++k)
			{
				size_t vertex0(region.borderVertices[j]);
				size_t vertex1(region.borderVertices[k]);
				ConnectEdge(vertex0,vertex1);
				ConnectEdge(vertex1,vertex0);
			}
		}
	}
}

static float ManhattanDistance(const PfVec2 & a, const PfVec2 & b)
{
	static const float straightDistance(1.0f);
	static const float diagonalDistance(1.5f);

	const int dx( abs(a.x-b.x) );
	const int dy( abs(a.y-b.y) );

	if(dx > dy)
	{
		return (dx - dy) * straightDistance + dy * diagonalDistance;
	}
	else
	{
		return (dy - dx) * straightDistance + dx * diagonalDistance;
	}
}

void PfMap::ConnectEdge(size_t i, size_t j)
{
	Edge & edge(GetEdge(i,j));
	edge.connected	= true;
	edge.cost		= ManhattanDistance(GetBorder(i).center, GetBorder(j).center);
	edge.successor	= j;
}

void PfMap::SetLine(const PfSegment & line)
{
	PfVec2 startSpace;
	PfVec2 endSpace;
	bool started(false);

	PfVec2 v0(line.v0);
	PfVec2 v1(line.v1);
	PfVec2 D(v1-v0);
	bool steep(abs(D.y) >= abs(D.x));
	if(steep) 
	{
		std::swap(v0.x,v0.y);
		std::swap(v1.x,v1.y);
		std::swap(D.x,D.y);
	}

	int xstep = 1;
	if (D.x < 0) 
	{
	    xstep = -1;
	    D.x = -D.x;
	}

	int ystep = 1;
	if (D.y < 0) 
	{
	    ystep = -1;		
	    D.y = -D.y; 
	}
	const int Dy2(D.y*2);
	const int Dy2Dx2(Dy2-D.x*2);
	int E(Dy2-D.x);
	int y(v0.y);
	for (int x = v0.x; x != v1.x; x += xstep) 
	{		
		const PfVec2 coords(steep ? PfVec2(y,x) : PfVec2(x,y));
		Tile & tile(At(coords.x,coords.y));
		if(!started && tile == TILE_SPACE)
		{
			startSpace = coords;
			started = true;
		}
		if(started)
		{
			if(tile == TILE_SPACE)
			{
				tile = TILE_BORDER;
				endSpace = coords;
			}
			else if(tile == TILE_OBSTRUCTION)
			{
				Border border;
				border.start	= startSpace;
				border.end		= endSpace;
				border.regionA	= TILE_SPACE;
				border.regionB	= TILE_SPACE;
				borders.push_back(border);
				started = false;
			}
		}

		if (E > 0) 
		{
			E += Dy2Dx2;
			y = y + ystep;
		} 
		else 
		{
			E += Dy2;
		}
	}
}

void PfMap::FloodFill(int x, int y, Tile region)
{
	std::deque<PfVec2> open;
	open.push_back(PfVec2(x,y));
	while(!open.empty())
	{
		PfVec2 coords(open.front()); open.pop_front();
		Tile & tile((*this)(coords.x,coords.y));
		if(tile == TILE_SPACE)
		{
			tile = region;
			open.push_back(coords+PfVec2(-1,0));
			open.push_back(coords+PfVec2( 1,0));
			open.push_back(coords+PfVec2(0,-1));
			open.push_back(coords+PfVec2(0, 1));
		}
	}
}

void PfMap::GenBorder(size_t index)
{
	const static PfVec2 dirs[] = {
		PfVec2(-1,-1),PfVec2( 0,-1),PfVec2( 1,-1),PfVec2( 1, 0),
		PfVec2( 1, 1),PfVec2( 0, 1),PfVec2(-1, 1),PfVec2(-1, 0)};

	Border & border(GetBorder(index));
	border.center = PfVec2(0,0);
	border.regionA = TILE_SPACE;
	border.regionB = TILE_SPACE;
	int count(0);

	std::deque<PfVec2> open;
	open.push_back(border.start);
	while(!open.empty())
	{
		PfVec2 coords(open.front()); open.pop_front();
		Tile & tile(At(coords.x,coords.y));
		if(tile == TILE_BORDER)
		{
			border.center = border.center + coords;
			++count;

			tile = TILE_SPACE;
			for(int index(0); index<8; ++index)
			{
				open.push_back(coords+dirs[index]);
			}
		}
		else if(tile != TILE_SPACE && tile != TILE_OBSTRUCTION)
		{
			if(border.regionA == TILE_SPACE)
			{
				border.regionA = tile;
				border.regionB = tile;
			}
			else if(tile != border.regionA)
			{
				border.regionB = tile;
			}
		}
	}

	border.center.x /= count;
	border.center.y /= count;

	open.clear();
	open.push_back(border.start);
	while(!open.empty())
	{
		PfVec2 coords(open.front()); open.pop_front();
		Tile & tile(At(coords.x,coords.y));
		if(tile == TILE_SPACE)
		{
			tile = border.regionA;
			for(int index(0); index<8; ++index)
			{
				open.push_back(coords+dirs[index]);
			}
		}
	}
}